/*
** polygon containment based on the paper 
** Chazelle, Bernard. The polygon containment problem. Carnegie-Mellon University, Department of Computer Science, 1981.
*/
#ifndef _POLYGON_CONTAINMENT_
#define _POLYGON_CONTAINMENT_

#include <algorithm>
#include <functional>
#include <exception>
#include <numeric>
#include <cmath>
#include <utility>
#include <list>
#include <string>

#include <boost/math/constants/constants.hpp>
#include <CGAL/Aff_transformation_2.h>
#include <CGAL/intersections.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Kernel/global_functions.h>
#include <CGAL/centroid.h>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_linalg.h>

//#include "../common/polytope.h"

namespace Field_packing
{
	typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
	typedef CGAL::Polygon_2<K>	CGAL_polygon;
	//using namespace Packing_common;
	// quadratic form function for two variable
	class Quadratic_function 
	{
	public:
		class Degenerate_quadratic_form : public std::exception
		{
		public:
			const char* what() const { return "the coefficient of both squared term must be non zero!"; }
		};
	public:
		Quadratic_function(double A_ = 0.0, double B_ = 0.0, double C_ = 0.0, double D_ = 0.0, double E_ = 0.0, double F_ = 0.0):
			A(A_), B(B_), C(C_), D(D_), E(E_), F(F_) { if (A*B == 0.0) throw Degenerate_quadratic_form(); }

		// f(alpha, beta) = A*alpha^2 + B*beta^2 + C*alpha*beta + D*alpha + E*beta + F
		inline double operator()(double x, double y) const  
		{
			return A*x*x + B*y*y + C*x*y + D*x + E*y + F;
		}
		inline void gradient(double x, double y, double& gx, double& gy) const
		{
			gx = 2*A*x + C*y + D;
			gy = 2*B*y + C*x + E;
		}
		// compute the extremum when min_alpha <= alpha <= max_alpha, min_beta <= beta <= max_beta
		void extrema(double min_x, double max_x, double min_y, double max_y, double& fmin, double& fmax) const;

	private:
		inline bool within_range(double lower, double upper, double val) const { return (val >= lower) && (val <= upper); }

	private:
		double A, B, C, D, E, F;
	};

	class Polygon_containment
	{

		typedef CGAL_polygon::Point_2	Point_2;
		typedef CGAL_polygon::Segment_2 Segment_2;
		typedef CGAL::Vector_2<CGAL_polygon::Traits>		Vector_2;
		typedef CGAL::Aff_transformation_2<CGAL_polygon::Traits> Transformation_2;
		typedef CGAL::Line_2<CGAL_polygon::Traits>	Line_2;
		typedef CGAL::Ray_2<CGAL_polygon::Traits> Ray_2;

	/* helper classes */
	public:
		class Containment_exception : public std::exception
		{
		public:
			Containment_exception(const std::string& message = "Containment Exception!") : mssg(message) {} 
			const char* what() const { return mssg.c_str(); }
		private:
			std::string mssg;
		};

	private:
		class Local_frame
		{
		public:
			Local_frame(const Point_2& orig, const Vector_2& x_axis, const Vector_2& y_axis) : o(orig), u(x_axis), v(y_axis) 
			{
				u = u / std::sqrt(u.squared_length());
				v = v / std::sqrt(v.squared_length());
			}
			Point_2 to_local_coord(const Point_2& p) const
			{
				Vector_2 op(o, p);
				return Point_2(op*u, op*v);
			}
			Segment_2 to_local_coord(const Segment_2& s) const
			{
				return Segment_2(to_local_coord(s.source()), to_local_coord(s.target()));
			}
			Vector_2 to_local_coord(const Vector_2& vec) const
			{
				return Vector_2(to_local_coord(CGAL::ORIGIN), to_local_coord(Point_2(vec.x(), vec.y())));
			}
			Point_2 to_world_coord(const Point_2& p) const
			{
				Transformation_2 to_world(u.x(), v.x(), o.x(), u.y(), v.y(), o.y());
				return to_world(p);
			}
			Vector_2 u_axis() const { return u; }
			Vector_2 v_axis() const { return v; }
		private:
			Point_2 o;
			Vector_2 u;
			Vector_2 v;
		};

	public:
		// Precondition: exterior and interior polygon must have the same orientation
		Polygon_containment(const CGAL_polygon& exterior_, const CGAL_polygon& interior_) : exterior(exterior_), interior(interior_) { }

		// find a containment configuration
		bool one_containment_copy(CGAL_polygon& placed_polygon);
		
		// find all containment copies
		int all_containment_copies(std::list<CGAL_polygon>& placed_polygons);
		
		//void detect_contacts(const CGAL_polygon& support_pgn, const CGAL_polygon& embed_pgn, std::list<CGAL_polygon>& placed_polygons);
		void contact_polygons(std::list<CGAL_polygon>& placed_polygons);

	private:
		// return true if the exterior can contain interior, without intersection
		bool has_inclusion(const CGAL_polygon& out_pgn, const CGAL_polygon& in_pgn);
		
		// detect all contacts. return all the placements that can make interior have three contacts with exterior
		void detect_contacts(const CGAL_polygon& support_pgn, const CGAL_polygon& embed_pgn, std::list<CGAL_polygon>& placed_plygons);
			
		// once one contact is detected, we check IMMEDIATELY if this contact can give a containment
		// this version is used in the case where it is fine to have a containment, no need to compute all containment configurations
		bool detect_contacts_and_check_inclusion(const CGAL_polygon& support_pgn, const CGAL_polygon& embed_pgn, bool support_is_exterior, CGAL_polygon& placed_polygon);
		
		// compute transformation corresponding three contacts
		// the version is problemetic, deprecated. see the backup.txt for recover
		bool determine_transformation(const Local_frame& lf, const Segment_2& fixed_edge_1, const Segment_2& fixed_edge_2, const Segment_2& AB, const Vector_2& Ox, double theta, double xalpha, Transformation_2& t);
		// the version that works
		bool determine_transformation(const Local_frame& lf, const CGAL_polygon& embed_pgn, const Segment_2& fixed_edge_1, const Segment_2& fixed_edge_2, const Point_2& A, const Point_2& B, double theta, double alpha, CGAL_polygon& transformed_polygon);
		// check whether a segment S can be moved along another segment T such that two end points are on T
		bool is_clamped(const Segment_2& fixed_edge_1, const Segment_2& fixed_edge_2, const Segment_2& moved_edge);

		// detect existence of type I contact while A and B are moving along some edge, i.e. intersection of trajectory of C with other edges 
		int type_I_contact_detect(
								 const Segment_2& AB, const Segment_2& BC, const Segment_2& PQ, double alpha,  
								 double& root_1, double& root_2
								 );

		// detect existence of type II contact while A and B are moving along some edge, i.e. intersection of trajectory of CC' with w
		int type_II_contact_detect(
									const Local_frame& lf, const Point_2& w, const Segment_2& AB, const Segment_2& dynamic_edge, 
									double alpha, std::list<double>& roots
								   );

		// helper functions
		// solve sine function equation sin(x) = c within the range a <= x <= b, where b - a <= pi, return number of solutions
		static int solve_sine(double a, double b, double c, double& root_1, double& root_2);
		// Newton's method to solve nonlnear equation, return computed root and number of iterations
		static std::pair<double, size_t> solve_eqn(std::function<double(double)> func_val, std::function<double(double)> diff_val, double init_x, double stop_error = 1e-8, size_t max_iter_nb = 10000);
		// compute the phase phi after transforming sin_coeff*sin(theta) + cos_coeff*cos(theta) to sqrt(sin_coeff*sin_coeff + cos_coeff*cos_coeff)*sin(theta + phi);
		static inline double lincomb_phi(double sin_coeff, double cos_coeff)
		{
			double phi;
			if (sin_coeff == 0.0 && cos_coeff > 0)
				phi = pi/2;
			else if (sin_coeff == 0.0 && cos_coeff < 0)
				phi = -pi/2;
			else
			{
				phi = std::atan(cos_coeff/sin_coeff);
				if (sin_coeff < 0)
					phi += pi;
			}
			return phi;
		}

		static inline double cross_product(const Vector_2& v0, const Vector_2& v1)
		{
			return v0.x()*v1.y() - v0.y()*v1.x();
		}

		static inline void compute_sin_cos(const Vector_2& v0, const Vector_2& v1, double& sin_theta, double& cos_theta)
		{
			cos_theta = v0*v1/std::sqrt(v0.squared_length()*v1.squared_length());
			if (cos_theta < -1.0)	cos_theta = -1.0;
			else if (cos_theta > 1.0)	cos_theta = 1.0;
			sin_theta = std::sqrt(1.0 - cos_theta*cos_theta);
			if (cross_product(v0, v1) < 0)
				sin_theta = -sin_theta;
		}

	private:
		const CGAL_polygon& exterior;
		const CGAL_polygon& interior;

		static const double pi;
	};
}

#endif